import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from img_msgs.msg import H26XFrame
from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data
from rclpy.qos import QoSReliabilityPolicy

# http://www.guyuehome.com/2388

class Listener(Node):
    def __init__(self, format = 'jpg'):
        super().__init__('phone_img_listener')
        # 创建一个subscriber，订阅名为chatter的topic
        # 消息类型是String，回调函数为chatter_callback
        self.format = format
        if format == "jpeg":
            self.sub = self.create_subscription(CompressedImage, '/image_raw/compressed', self.chatter_callback, qos_profile=qos_profile_system_default)
        else:
            self.sub = self.create_subscription(H26XFrame, '/image_raw/compressed/%s' % format, self.chatter_callback, qos_profile=qos_profile_system_default)
        self.save = False
        self.f = open('test.%s' % self.format, 'wb')

    def chatter_callback(self, msg):
        # 将收到的消息数据打印出来
        if self.format == "jpg":
            self.get_logger().info('I heard: [%d]' % msg.header.stamp.sec)
        else:
            self.get_logger().info('I heard: [%d, %d]' % (msg.width, msg.height))
        self.f.write(msg.data)

    def close_file():
        self.f.close()

def main(args=None):
    print('Hi from phone_control.')
    # ROS节点初始化
    rclpy.init(args=args)
    # 创建节点及订阅器
    node = Listener('h265')
    # 循环查询消息队列，收到消息后进入回调函数
    rclpy.spin(node)
    # 销毁节点，退出程序
    node.destroy_node()
    node.close_file()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
